
// 锥形放射源高：0.28m
// 锥顶长：0.315m，宽：0.175m
// 放射源中心检测原理：提取固定高度锥形放射源点云，将该段点云压成2d后，对其进行拟合；
//                 计算拟合后的长方形的中心，即为中心


#include "back_center_detection/gamma_detector.hpp"
#include <algorithm>
#include <cmath>

namespace back_center_detection {

    void GammaDetector::setParameters(rclcpp::Node* node) {

        node->get_parameter("gamma.search_start_z", params_.search_start_z);
        node->get_parameter("gamma.search_end_z", params_.search_end_z);
        node->get_parameter("gamma.search_step", params_.search_step);
        node->get_parameter("gamma.height_range", params_.height_range);
        node->get_parameter("gamma.min_points_in_slice", params_.min_points_in_slice);
        node->get_parameter("gamma.gamma_height", params_.gamma_height);
        node->get_parameter("gamma.rect_min_area", params_.rect_min_area);
        node->get_parameter("gamma.rect_max_area", params_.rect_max_area);

    }

    bool GammaDetector::detectCenter(PointCloud cloud,
                                    geometry_msgs::msg::PointStamped& center_point) {
                
        if(cloud->empty()) {
            RCLCPP_WARN(rclcpp::get_logger("gamma_detector"), "Empty cloud for gamma detection!");
            return false;
        }

        // 1.检测顶部区域
        float detected_z = NAN;
        auto gamma_cloud = detectTopRegion(cloud, detected_z);
        if(std::isnan(detected_z)) {
            RCLCPP_WARN(rclcpp::get_logger("gamma_detector"), "No top gamma cloud found!");
            return false;
        }
        if(static_cast<int>(gamma_cloud->size()) < params_.min_points_in_slice) {
            RCLCPP_WARN(rclcpp::get_logger("gamma_detector"), 
                                          "Not enough points out of %ld!", gamma_cloud->size());
            return false;
        }

        // 2.投影到XY平面
        auto points_2d = projectTo2D(gamma_cloud);

        //3.拟合外框矩形
        cv::Point2f rect_center;
        if(!fitRectangleAndGetCenter(points_2d, rect_center)) {
            RCLCPP_WARN(rclcpp::get_logger("gamma_detector"), 
                                          "Failed to fit outer rectangle");
            return false;
        }


        //4.设置结果
        center_point.point.x = rect_center.x;
        center_point.point.y = rect_center.y;
        center_point.point.z = detected_z;
        center_point.header.frame_id = "map";

        RCLCPP_INFO(rclcpp::get_logger("gamma_detector"),
                    "Gamma Detector center: %.3f, %.3f, %.3f",
                    rect_center.x, rect_center.y, detected_z);
        return true;
        
    }



    GammaDetector::PointCloud GammaDetector::detectTopRegion(const PointCloud& cloud,
                                                                float& detected_z) {
        auto gamma_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
        detected_z = NAN;

        //从上往下开始搜索
        for(float current_z = params_.search_start_z; 
            current_z > params_.search_end_z;
            current_z -= params_.search_step) {
            gamma_cloud->clear();

            //提取当前高度切片
            for(const auto& point : *cloud) {
                if(point.z >= current_z && 
                   point.z <= current_z + params_.height_range) {
                    gamma_cloud->push_back(point);
                   }
            }
            if(static_cast<int>(gamma_cloud->size()) > params_.min_points_in_slice) {
                detected_z = params_.gamma_height;
                RCLCPP_INFO(rclcpp::get_logger("gamma_detector"), 
                           "Found top region at z=%.3f with %ld points", 
                           detected_z, gamma_cloud->size());
                break;
            }
        }

        return gamma_cloud;
    }


    std::vector<cv::Point2f> GammaDetector::projectTo2D(const PointCloud& cloud) {
        std::vector<cv::Point2f> points_2d;
        points_2d.reserve(cloud->size());

        for(const auto& point : *cloud) {
            points_2d.emplace_back(point.x, point.y);
        }
        return points_2d;
    }


    bool GammaDetector::fitRectangleAndGetCenter(const std::vector<cv::Point2f>& points,
                                                cv::Point2f& center) {
        if(points.size() < 4) {
            return false;
        }

        //使用opencv拟合最小外接矩形
        cv::RotatedRect rect = cv::minAreaRect(points);
        

        //验证矩形面积
        float area = rect.size.width * rect.size.height;
        if(area < params_.rect_min_area || area > params_.rect_max_area) {
            RCLCPP_DEBUG(rclcpp::get_logger("gamma_detector"), 
                        "Rectangle area out of range: %.4f", area);
            return false;
        }

         
        //返回矩阵中心坐标
        center.x = rect.center.x;
        center.y = rect.center.y;

        RCLCPP_INFO(rclcpp::get_logger("gamma_detector"), 
                   "Fitted rectangle: center(%.3f, %.3f), size(%.3f, %.3f), area=%.4f",
                   rect.center.x, rect.center.y, rect.size.width, rect.size.height, area);
        

        return true;
    }
}



